/*****************************************************************//**
 * \file   RobotControlInterface.cpp
 * \brief  
 * 
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/

#include "RobotControlInterface.h"

#if 0
/**
 * \brief   .
 * 
 */
RobotControlInterface::RobotControlInterface()
{
	isArmInit = false;
	isToolInit = false;

	//Franka2坐标系转到Aobo2坐标系
	franka2aobo2 << -0.9957, 0.0357, 0.0917, 1.1165,
		-0.0334, -0.9792, -0.1960, -0.6504,
		0.0868, -0.2000, 0.9761, -0.1618,
		0, 0, 0, 1;

	//Aobo1坐标系转到Aobo2坐标系
	aobo12aobo2 << -0.9998, -0.0006, -0.0171, 0.0122,
		0.0128, -0.9999, 0.0152, -0.6826,
		-0.0173, 0.0152, 0.9997, 0.0022,
		0, 0, 0, 1;
}

/**
 * \brief   .
 * 
 */
RobotControlInterface::~RobotControlInterface()
{
	
}

/**
 * \brief   .
 * 
 * \param robotNumber
 * \return 
 */
bool RobotControlInterface::InitRobot(int robotType,int robotNumber, const char* ipAddr)
{
	switch (robotType) {
	case ARMTYPE_AOBOi5:
		pAuboRobot[robotNumber] = new AuboRobotControl();
		if (!pAuboRobot[robotNumber]->robotLogin(ipAddr, AuboRobotNetPort))
		{
			std::cout << "Robot" << robotNumber << "login failed!" << std::endl;
			pAuboRobot[robotNumber]->bInitDone = false;
			isArmInit = false;
			return false;
		}
		else {

			pAuboRobot[robotNumber]->bInitDone = true;
			isArmInit = true;
		}
		break;
	case ARMTYPE_FRANKA:
		pFrankaRobot[robotNumber] = new FrankaRobotControl();
		if (!pFrankaRobot[robotNumber]->robotLogin(ipAddr))
		{

			std::cout << "Robot" << robotNumber << "login failed!" << std::endl;
			pFrankaRobot[robotNumber]->bInitDone = false;
			isArmInit = false;
			return false;
		}
		else {

			pFrankaRobot[robotNumber]->bInitDone = true;
			isArmInit = true;
		}
		break;
	case ARMTYPE_UR3:
		break;
	default:
		return false;
	}

	return true;
}

/**
 * \brief   .
 * 
 * \param EndToolName
 * \return 
 */
bool RobotControlInterface::InitEndTool(int EndToolName)
{
	//switch (EndToolName) {
	//case EndCamera:
	//	EndTool[EndToolName] = new CToolModel_Camera();
	//	break;
	//case EndBoplarHook:
	//	EndTool[EndToolName] = new CToolModel_BoplarHook();
	//	break;
	//case End3Dof:
	//	EndTool[EndToolName] = new CToolModel_Tradition();
	//	break;
	//case End4Dof:
	//	EndTool[EndToolName] = new CToolModel_4Dof();
	//	break;
	//default:
	//	return false;
	//}

	//EndTool[EndToolName]->isEndToolInit = true;

	isToolInit = true;

	return true;
}

/**
 * \brief   .
 * 
 * \param robotNumber
 * \param pPos
 */
void RobotControlInterface::MoveArm(int robotType, int robotNumber, double* pPos)
{
	if (ARMTYPE_AOBOi5 == robotType) 
	{
		if ((pAuboRobot[robotNumber] != nullptr) &&(pAuboRobot[robotNumber]->bInitDone) )
		{
			pAuboRobot[robotNumber]->robotMoveJ(pPos);
		}
		else { 
			std::cout << "This robot:" << robotNumber << "may be not exist!" << std::endl;
			return;
		}
	}
	else if(ARMTYPE_FRANKA == robotType)
	{
		if ((pFrankaRobot[robotNumber] != nullptr) && (pFrankaRobot[robotNumber]->bInitDone))
		{
			//Franka 目前没有对应的接口
			//pFrankaRobot[robotNumber]->robotMoveJ(pPos);

		}
		else {
			std::cout << "This robot:" << robotNumber << "may be not exist!" << std::endl;
			return;
		}
	}
	else if (ARMTYPE_UR3 == robotType) 
	{
		//do something
	}

	return;
}

/**
 * \brief   .
 * 
 * \param robotType
 * \param robotNumber
 * \return 
 */
bool RobotControlInterface::GetRobotConnectStatus(int robotType, int robotNumber)
{
	bool bConnectStatus = false;

	if (ARMTYPE_AOBOi5 == robotType)
	{
		if ((pAuboRobot[robotNumber] != nullptr) && (pAuboRobot[robotNumber]->bInitDone))
		{
			bConnectStatus = pAuboRobot[robotNumber]->robotGetConnectState();
		}
		else {
			std::cout << "This robot:" << robotNumber << "may be not exist!" << std::endl;
			return false;
		}
	}
	else if (ARMTYPE_FRANKA == robotType)
	{
		if ((pFrankaRobot[robotNumber] != nullptr) && (pFrankaRobot[robotNumber]->bInitDone))
		{
			bConnectStatus = pFrankaRobot[robotNumber]->robotGetState();
		}
		else {
			std::cout << "This robot:" << robotNumber << "may be not exist!" << std::endl;
			return false;
		}
		
	}
	else if (ARMTYPE_UR3 == robotType)
	{
		//do something
	}
	else {
		std::cout << "The robot type: " << robotType << "may be wrong." << std::endl;
		return false;
	}

	return bConnectStatus;
}

/**
 * \brief   .
 * 
 * \param robotType
 * \param robotNumber
 * \return 
 */
void RobotControlInterface::GetRobotJointStatus(int robotType, int robotNumber, sRobotRealTimeDataCtrl&sRunTimedata)
{
	

	//机械臂各个关节角
	JointStatus jointStatus[6] = { 0 };
	//机械臂末端姿态
	Eigen::Matrix3d rotationMatrix;
	//机械臂末端位置
	Eigen::Vector3d positionVector;

	if (ARMTYPE_AOBOi5 == robotType)
	{
		if ((pAuboRobot[robotNumber] != nullptr) && (pAuboRobot[robotNumber]->bInitDone))
		{
			pAuboRobot[robotNumber]->robotGetJointStatus(jointStatus);
			
			for (int i = 0; i < 6; i++) {
				sRunTimedata.sRobotDy.dCoo6FJoint[i] = (double)jointStatus[i].jointPosJ;
			}

			pAuboRobot[robotNumber]->robotForward(sRunTimedata.sRobotDy.dCoo6FJoint, rotationMatrix, positionVector);
			sRunTimedata.sRobotDy.EndMatrix.block(0, 0, 3, 3) = rotationMatrix;
			sRunTimedata.sRobotDy.EndMatrix.block(0, 3, 3, 1) = positionVector;

			sRunTimedata.sRobotDy.orientation = sRunTimedata.sRobotDy.EndMatrix.block(0, 0, 3, 3);
			sRunTimedata.sRobotDy.position = sRunTimedata.sRobotDy.EndMatrix.block(0, 3, 3, 1);
			return;
			
		}
	}
	else if (ARMTYPE_FRANKA == robotType)
	{
		sRunTimedata.sRobotDy.EndMatrix = pFrankaRobot[robotNumber]->robotReadp();

		sRunTimedata.sRobotDy.position = sRunTimedata.sRobotDy.EndMatrix.block(0, 3, 3, 1);
		sRunTimedata.sRobotDy.orientation = sRunTimedata.sRobotDy.EndMatrix.block(0, 0, 3, 3);
		sRunTimedata.sRobotDy.dPosturePx = sRunTimedata.sRobotDy.position(0);
		sRunTimedata.sRobotDy.dPosturePy = sRunTimedata.sRobotDy.position(1);
		sRunTimedata.sRobotDy.dPosturePz = sRunTimedata.sRobotDy.position(2);
		return;
	}
	else if (ARMTYPE_UR3 == robotType)
	{
		//do some thing
	}
	else {
		std::cout << "May be the robot type:" << robotType << "is wrong" << std::endl;
	}


	return;
}

/**
 * \brief   .
 * 
 * \param robotType
 * \param robotNumber
 * \return 
 */
void RobotControlInterface::GetRealtimeState(int robotType, int robotNumber, sRobotRealTimeDataCtrl&sRunTimedata)
{
	GetRobotJointStatus(robotType, robotNumber, sRunTimedata);
	return;
}

// 这个应该放入算法里面
void RobotControlInterface::HapticControlRobot(int robotType,int robotNumber, int EndToolName, Eigen::Matrix4d mat4dNext)
{
	if (ARMTYPE_AOBOi5 == robotType)
	{
		if ((pAuboRobot[robotNumber] != nullptr) && (pAuboRobot[robotNumber]->bInitDone))
		{
			//pAuboRobot[robotNumber]->Move(mat4dNext);
		}
	}
	else if (ARMTYPE_FRANKA == robotType)
	{
		//do some thing
	}
	else if (ARMTYPE_UR3 == robotType)
	{
		//do some thing
	}
	else {
		std::cout << "May be the robot type:" << robotType << "is wrong" << std::endl;
	}
	return;
}

// 这个应该放入算法里面
void RobotControlInterface::HapticControlRobot_moveJ(int robotType, int robotNumber, Eigen::Matrix4d robotmat/*Eigen::Matrix3d orientation_matrix, Eigen::Vector3d transposition*/)
{
	double target_postion[6];

	if (ARMTYPE_AOBOi5 == robotType) {
		//robotmat.block(0, 0, 3, 3) = orientation_matrix;
		//robotmat.block(0, 3, 3, 1) = transposition;
		if (pAuboRobot[robotNumber] != nullptr && pAuboRobot[robotNumber]->bInitDone) {
			//pAuboRobot[RobotName]->MoveJ(robotmat);
			pAuboRobot[robotNumber]->robotInverse(robotmat, target_postion);
			pAuboRobot[robotNumber]->robotMoveJ(target_postion);
		}
	}

	return;
}


Eigen::Matrix4d RobotControlInterface::Robot1402RobotBase(Eigen::Matrix4d robot140_end)
{
	return robot140_end * aobo12aobo2 * franka2aobo2.inverse();
}

Eigen::Matrix4d RobotControlInterface::Robot1382RobotBase(Eigen::Matrix4d robot138_end)
{
	return robot138_end * franka2aobo2.inverse();
}

/**
 * \brief   .
 * 
 * \param robotType
 * \param robotNumber
 */
void RobotControlInterface::RobotControlStop(int robotType, int robotNumber)
{
	if (ARMTYPE_AOBOi5 == robotType)
	{
		if ((pAuboRobot[robotNumber] != nullptr) && (pAuboRobot[robotNumber]->bInitDone))
		{
			pAuboRobot[robotNumber]->robotShutdown();
		}
	}
	else if (ARMTYPE_FRANKA == robotType)
	{
		if ((pFrankaRobot[robotNumber] != nullptr) && (pFrankaRobot[robotNumber]->bInitDone))
		{
			pFrankaRobot[robotNumber]->robotShutdown();
		}
	}
	else if (ARMTYPE_UR3 == robotType)
	{
		//do some thing
	}
	else {
		std::cout << "May be the robot type:" << robotType << "is wrong" << std::endl;
	}

	isArmInit = false;
	isToolInit = false;

	return;
}

/**
 * \brief   .
 * 
 * \param RobotName
 */
void RobotControlInterface::GetToolEnd(int RobotName)
{
	//if (ControlRobot[RobotName] != nullptr && EndTool[RobotName] != nullptr) {
	//	g_RobotRealTimeData[RobotName].motorDevice.Toolend_matrix = EndTool[RobotName]->geToolEndMatrix(g_RobotRealTimeData[RobotName].robotDy.EndMatrix);
	//	g_RobotRealTimeData[RobotName].robotDy.tool_posture = g_RobotRealTimeData[RobotName].motorDevice.Toolend_matrix.block(0, 3, 3, 1);
	//	g_RobotRealTimeData[RobotName].robotDy.tool_px = g_RobotRealTimeData[RobotName].motorDevice.Toolend_matrix(0, 3);
	//	g_RobotRealTimeData[RobotName].robotDy.tool_py = g_RobotRealTimeData[RobotName].motorDevice.Toolend_matrix(1, 3);
	//	g_RobotRealTimeData[RobotName].robotDy.tool_pz = g_RobotRealTimeData[RobotName].motorDevice.Toolend_matrix(2, 3);
	//}
	return;
}
#endif

